#include <rclcpp/rclcpp.hpp>
// #include <simple_action_server.hpp>
#include <turtle_bt_ros2/action/bt.hpp>
#include "rclcpp_action/rclcpp_action.hpp"
#include "behaviortree_cpp_v3/action_node.h"

#include <tf2/exceptions.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/twist.hpp>
# include <turtle_bt_ros2/srv/spawn.hpp>

#include <chrono>
#include <memory>
#include <string>

using std::placeholders::_1;
using namespace std::chrono_literals;
static int flag3 = 0;


class NavEnemy : public BT::ActionNodeBase {
public:
    explicit NavEnemy(const std::string &name, const BT::NodeConfiguration &config):
        BT::ActionNodeBase(name, config) {
        if (flag3 == 0)  {
            node_ = rclcpp::Node::make_shared(name);
            tf_buffer_nav = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
            transform_listener_nav = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_nav);
            flag3 = 1;
        }


        publisher_nav = node_->create_publisher<geometry_msgs::msg::Twist>("/turtle2/cmd_vel", 10);
    }

    NavEnemy() = delete;

    ~NavEnemy() {
        RCLCPP_DEBUG(node_->get_logger(), "Shutting down NavEnemy BT node");
    }

    static BT::PortsList providedPorts() {
        return { BT::OutputPort < std::string > ("nav  enemy") };
    }

    BT::NodeStatus tick() override {
        if (execute_callback()) {
            RCLCPP_INFO(node_->get_logger(), "Action nav_enemy is successful!");
            return BT::NodeStatus::RUNNING;
        } else {
            RCLCPP_INFO(node_->get_logger(), "Everything is ok."); //永远进不来
            return BT::NodeStatus::FAILURE;
        }
    }

    void halt() override {
        std::cout << name() << ": Halted." << std::endl;
    }
    
    bool execute_callback() {
        // tf_buffer_nav = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
        // transform_listener_nav = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_nav);

        RCLCPP_INFO(node_->get_logger(), "Starting Condition nav_enemy");

        geometry_msgs::msg::TransformStamped transformStamped;

        rclcpp::WallRate loop_rate(5);
        loop_rate.sleep();

        transformStamped = tf_buffer_nav->lookupTransform("turtle2", "turtle1", tf2::TimePointZero);

        geometry_msgs::msg::Twist vel_msg;
        vel_msg.angular.z = 1.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);
        vel_msg.linear.x = 1.0;
        publisher_nav->publish(vel_msg);
        return true;
    }
private:
    rclcpp::Node::SharedPtr node_;
    std::shared_ptr<tf2_ros::TransformListener> transform_listener_nav{nullptr};
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_nav;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_nav{nullptr};
};